//陀螺仪模块程序
void gyroscope_init(){
  // Try to initialize!
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  //MPU6050 支持的加速度计量程有 ±2g、±4g、±8g 和 ±16g，这里设置为 ±16g，
  //意味着传感器可以测量的最大加速度为 16 倍的重力加速度
  mpu.setAccelerometerRange(MPU6050_RANGE_16_G);
  //MPU6050 支持的陀螺仪量程有 ±250 度 / 秒、±500 度 / 秒、±1000 度 / 秒
  //和 ±2000 度 / 秒，这里设置为 ±250 度 / 秒，意味着传感器可以测量的最大
  //角速度为 250 度 / 秒。
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  //mpu.setFilterBandwidth()函数用于设置数字低通滤波器的带宽。这个带宽决定了
  //传感器输出数据的平滑程度。较低的带宽可以减少噪声，但可能会牺牲一些响应速度。
  //这里设置为 21Hz，意味着滤波器将允许低于 21Hz 的信号通过。
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  Serial.println("");
  delay(100);
}


//读取原始加速度计和陀螺仪值,并计算滚转，俯仰角
void get_gyroscope(){
  /* Get new sensor events with the readings */
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  roll = atan2(a.acceleration.x, a.acceleration.z) * 180 / PI;
  pitch = atan2(-a.acceleration.y, sqrt(a.acceleration.x * a.acceleration.x + a.acceleration.z * a.acceleration.z)) * 180 / PI;
  static float last_gyro_roll = 0;
  static float last_gyro_pitch = 0;
  static float last_gyro_yaw = 0;

  float gyro_yaw = g.gyro.z * 180 / PI; // 将角速度转换为度/秒
  yaw += gyro_yaw * 0.01; // 假设采样周期为10ms，将角速度积分得到偏航角变化量

  // Serial.print("Roll:"); Serial.print(roll);Serial.print(",");
  // Serial.print("pitch:"); Serial.print(pitch);Serial.print(",");
  // Serial.print("yaw:"); Serial.println(yaw);

  delay(10);
}



//如果不知道iic设备地址，可以调用下面的函数查询到iic设备地址
void search_for_iic_address(){
  byte error, address;
  int nDevices;
  Serial.println("Scanning...");
  nDevices = 0;
  for(address = 1; address < 127; address++ ) 
  {
    // The i2c_scanner uses the return value of
    // the Write.endTransmisstion to see if
    // a device did acknowledge to the address.
    Wire.beginTransmission(address);
    error = Wire.endTransmission();
    if (error == 0)
    {
      Serial.print("I2C device found at address 0x");
      if (address<16) 
        Serial.print("0");
      Serial.print(address,HEX);
      Serial.println("  !");
 
      nDevices++;
    }
    else if (error==4) 
    {
      Serial.print("Unknown error at address 0x");
      if (address<16) 
        Serial.print("0");
      Serial.println(address,HEX);
    }    
  }
  if (nDevices == 0)
    Serial.println("No I2C devices found\n");
  else
    Serial.println("done\n");
  delay(5000);           // wait 5 seconds for next scan
}